#include <cstdlib>
#include <iostream>
#include <random>
#include <Eigen/Core>
#include <vector>

#define PI 3.14159265359

class Particle{

public:

    Particle(float x, float y, float vx, float vy, int seed)
    {
        initParticle(x, y, vx, vy, seed);
    }

    ~Particle() {}

    float getPositionX()
    {
        return this->position_x;
    }

    float getPositionY()
    {
        return this->position_y;
    }

    float getOrientation()
    {
        return this->orientation;
    }

    float getProbability()
    {
        return this->probability;
    }

    float setParameters(float x, float y, float vx, float vy, float theta)
    {
        this->position_x  = x;
        this->position_y = y;
        this->velocity_x = vx;
        this->velocity_y = vy;
        this->orientation = theta;
    }

    float setVelocity(float x, float y)
    {
        this->velocity_x = x + this->distribution(this->generator);
        this->velocity_y = y + this->distribution(this->generator);
    }

    void update_particle(float time)
    {
        this->position_x = this->position_x + this->velocity_x*time;
        this->position_y = this->position_y + this->velocity_y*time;
        this->velocity_x+=this->distribution(this->generator);//*(sqrt(time));
        this->velocity_y+=this->distribution(this->generator);//*(sqrt(time));
    }

    void determine_particle_weight(float x, float y, float vx, float vy)
    {
        float sigma_squared = 0.5;
        float distance = pow((this->position_x-x),2) + pow((this->position_y-y),2) + pow((this->velocity_y-vy),2) + pow((this->velocity_x-vx),2);
        this->probability = exp(-distance / (2*sigma_squared));
    }

private:

    float position_x;
    float position_y;
    float velocity_x;
    float velocity_y;
    float orientation;
    float probability;

    std::default_random_engine generator;
    std::normal_distribution<float> distribution;

    void initParticle(float x, float y, float vx, float vy, int seed)
    {
        this->generator.seed(seed);
        std::normal_distribution<float> temp(0.0,1.0);
        this->distribution = temp;
        this->position_x = x;// + this->distribution(this->generator);
        this->position_y = y;// + this->distribution(this->generator);
        this->velocity_x = vx + this->distribution(this->generator);
        this->velocity_y = vy + this->distribution(this->generator);
    }

};
